#include "helloworldagent.ih"
#include <Eigen/Core>
//#include <Eigen/Dense>

void HelloWorldAgent::think()
{


  // The Cerebellum collects actions to perform, integrating them where necesary
  Cerebellum& cer = SCerebellum::getInstance();

   WorldModel& wm = SWorldModel::getInstance();

  // The AgentModel keeps track of the state of the robot, e.g. joint angles,
  // location of COM, et cetera
  AgentModel& am = SAgentModel::getInstance();


  /*cer.addAction(new MoveJointAction(Types::LARM3, -0.01));
  cer.addAction(new MoveJointAction(Types::LARM4, -0.01));
  cer.addAction(new MoveJointAction(Types::LARM1, -0.01));
  cer.addAction(new MoveJointAction(Types::RARM3, 0.01));
  cer.addAction(new MoveJointAction(Types::RARM4, 0.01));
  cer.addAction(new MoveJointAction(Types::RARM1, -0.01));*/

  walk();

  // Tell Cerebellum to send the actions to the server
  cer.outputCommands(SAgentSocketComm::getInstance());

}
void HelloWorldAgent::walk()
{

  Cerebellum& cer = SCerebellum::getInstance();
      // The WorldModel keeps track of the state of the world, e.g. play mode, time,
  // players/opponents/ball positions, et cetera
  WorldModel& wm = SWorldModel::getInstance();

  // The AgentModel keeps track of the state of the robot, e.g. joint angles,
  // location of COM, et cetera
  AgentModel& am = SAgentModel::getInstance();

  // Get the current time
  double t = wm.getTime();
  // Get the current angles of the leg joints.
  double angles[12];
  angles[0] = am.getJoint(Types::LLEG2)->angle->getMu()(0);
  angles[1] = am.getJoint(Types::LLEG4)->angle->getMu()(0);
  angles[2] = am.getJoint(Types::LLEG5)->angle->getMu()(0);
  angles[3] = am.getJoint(Types::RLEG2)->angle->getMu()(0);
  angles[4] = am.getJoint(Types::RLEG4)->angle->getMu()(0);
  angles[5] = am.getJoint(Types::RLEG5)->angle->getMu()(0);
  angles[6] = am.getJoint(Types::RARM1)->angle->getMu()(0);
  angles[7] = am.getJoint(Types::LARM1)->angle->getMu()(0);
  angles[8] = am.getJoint(Types::LARM4)->angle->getMu()(0); //elbow
  angles[9] = am.getJoint(Types::RARM4)->angle->getMu()(0); //elbow
  angles[10] = am.getJoint(Types::RARM3)->angle->getMu()(0);
  angles[11] = am.getJoint(Types::LARM3)->angle->getMu()(0);

  // Determine target angles for joints
  double targets[12];
  //Need to manipulate joints 2,4,5 (hip, knee and ankle in X-axis).
  //Sinusoidal pattern generator:
  //Joint angle at t = Amplitude * sin( t * 2pi + phase offset) + constant offset

  //Phase offset creates forward movement.
  // TODO: Create time_interval variable for changing of time period, change back to 0.5

  float fpTime = 0.5;

  targets[0] = (60 * M_PI/180 + 16*M_PI/180 * sin(t / fpTime * 2*M_PI)) + (0 + -4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI/2)) + (0 + -4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI));
  targets[1] = (-50* M_PI/180 + -32*M_PI/180 * sin(t / fpTime * 2*M_PI));
  targets[2] = (18 * M_PI/180 + 16*M_PI/180 * sin(t / fpTime * 2*M_PI)) + (0 + -4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI/2)) + (0 + -4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI));
  targets[3] = (60 * M_PI/180 + 16*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI)) + (0 + -4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI*3/2)) + (0 + 4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI/2));
  targets[4] =  (-50* M_PI/180 + -32*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI));
  targets[5] = (18 * M_PI/180 + 16*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI)) + (0 + -4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI*3/2)) + (0 + 4*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI/2));

  targets[7] = (-100 * M_PI/180 + 48*M_PI/180 * sin(t / fpTime * 2*M_PI)) + (0 + -4* M_PI/180 * sin(t / fpTime * 2* M_PI + M_PI/2)); //+ (-0.1 + -4* M_PI/180 * sin( t / 0.5 * 2*M_PI + M_PI/2)); // sinusodial try for Left and right arms.
  targets[6] = (-100 * M_PI/180 + 48*M_PI/180 * sin(t / fpTime * 2*M_PI + M_PI)) + (0 + -4* M_PI/180 * sin(t / fpTime * 2* M_PI + M_PI*3/2));
  targets[8] = (-60 * M_PI/180 + 0*M_PI/180 * sin(t / fpTime * 2*M_PI));// + (0 + -4* M_PI/180 * sin(t / fpTime * 2* M_PI + M_PI/2));
  targets[9] = (60 * M_PI/180 + 0*M_PI/180 * sin(t / fpTime * 2*M_PI));// + (0 + -4* M_PI/180 * sin(t / fpTime * 2* M_PI + M_PI/2));
  targets[10] = (90 * M_PI/180 + 0*M_PI/180 * sin(t / fpTime * 2*M_PI));// + (0 + -4* M_PI/180 * sin(t / fpTime * 2* M_PI + M_PI/2));
  targets[11] = (-90 * M_PI/180 + 0*M_PI/180 * sin(t / fpTime * 2*M_PI));

    // Determine angular velocities needed to achieve these goal angles;
  double velocities[12];

  for (unsigned i = 0; i < 12; ++i)
  {
      velocities[i] = 0.1 * (targets[i] - angles[i]);
  }

  cer.addAction(new MoveJointAction(Types::LLEG2, velocities[0]));
  cer.addAction(new MoveJointAction(Types::LLEG4, velocities[1]));
  cer.addAction(new MoveJointAction(Types::LLEG5, velocities[2]));
  cer.addAction(new MoveJointAction(Types::LARM1, velocities[7]));

  cer.addAction(new MoveJointAction(Types::RLEG2, velocities[3]));
  cer.addAction(new MoveJointAction(Types::RLEG4, velocities[4]));
  cer.addAction(new MoveJointAction(Types::RLEG5, velocities[5]));
  cer.addAction(new MoveJointAction(Types::RARM1, velocities[6]));
  cer.addAction(new MoveJointAction(Types::LARM4, velocities[8]));
  cer.addAction(new MoveJointAction(Types::RARM4, velocities[9]));
  cer.addAction(new MoveJointAction(Types::RARM3, velocities[10]));
  cer.addAction(new MoveJointAction(Types::LARM3, velocities[11]));
}
